package es.uji.viselab.image.source;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.util.Iterator;

import javax.swing.JPanel;
import javax.vecmath.Vector3d;

import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.camera.CameraCatalog;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector2;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.model.ModelCreator;
import es.uji.viselab.robot.Robot;
import es.uji.viselab.util.LogLevel;
import es.uji.viselab.util.LogRecorder;
import es.uji.viselab.world.World;

public class ImageCameraSimulated extends ImageSource {

	public ImageCameraSimulated(ExperimentParameters ep) {
		super(ep);
	}

	JPanel panel;
	Matrix4 robotPose;
	protected boolean pointsCalculated = false;

	/*
	 * public ImageCameraSimulated(Experiment experiment) { super(experiment);
	 * this.modelParams = experiment.getModelParameters(); }
	 */

	private void paint() {

		Graphics g = panel.getGraphics();

		Graphics2D g2d = (Graphics2D) g;

		g2d.setColor(Color.red);

		// Target
		for (int i = 0; i < targetPointList.length; i++) {
			int u = (int) (targetPointList[i].x);
			int v = (int) (targetPointList[i].y);
			g2d.drawOval(u - 3, v - 3, 6, 6);
		}

		g2d.setColor(Color.blue);

		Iterator<Vector2[]> iterator = ImagePointListList.iterator();
		Vector2[] imagePointList = null;
		Vector2[] previousImagePointList = null;
		while (iterator.hasNext()) {
			imagePointList = iterator.next();
			for (int i = 0; i < imagePointList.length; i++) {
				int u = (int) (imagePointList[i].x);
				int v = (int) (imagePointList[i].y);
				g2d.drawOval(u - 3, v - 3, 6, 6);
			}
			if (previousImagePointList != null)
				for (int i = 0; i < imagePointList.length; i++) {
					int u0 = (int) (previousImagePointList[i].x);
					int v0 = (int) (previousImagePointList[i].y);
					int u1 = (int) (imagePointList[i].x);
					int v1 = (int) (imagePointList[i].y);
					g2d.drawLine(u0, v0, u1, v1);
				}
			previousImagePointList = imagePointList;

		}

	}

	private void calculateImageCoordinates(Matrix4 pose) {
		pointsCalculated = true;
		Vector3[] modelCoordinateList = ModelCreator.get3d(ep.getModel(), ep.getCamParameters(),
				ep.getTarget());
		imagePointList = convert3dTo2d(modelCoordinateList, pose, ep.getCamParameters());
		imageCoordinateList = convertUVtoXY(imagePointList);
	}

	@Override
	public void robotPositionChanged(Robot robot) {
		robotPose = robot.getPoseM();
		Matrix4 cameraPose = ep.getCamParameters().getPose();
		Matrix4 pose = new Matrix4();
		pose.mul(robotPose, cameraPose);
		calculateImageCoordinates(pose);
		if (this.panel != null) {
			// Vector2[] points = new Vector2[this.imagePointList.length]();
			Vector2[] points = this.imagePointList;
			ImagePointListList.add(points);
			paint();
		}
		notifyFeaturePointsChange(this);
	}

	private Vector2[] convert3dTo2d(Vector3[] worldPoints3d, Matrix4 pose, CameraCatalog camParams) {
		Matrix4 ipose = pose.getInverseHomogenousTransformation();
		Vector2[] imagePixel = new Vector2[worldPoints3d.length];
		double fd = camParams.getFocalDistanceCorrected();
		for (int i = 0; i < worldPoints3d.length; i++) {
			// Move the 3D points to the camera coordinate system
			Vector3d imagePoint3d = ipose.mul(worldPoints3d[i]);

			double x = (imagePoint3d.x / imagePoint3d.z) * fd;
			double y = (imagePoint3d.y / imagePoint3d.z) * fd;
			double u = (camParams.getImageWidth() / 2 + x);
			double v = camParams.getImageHeight() / 2 + (y * -1);
			imagePixel[i] = new Vector2((int) u, (int) v);
		}
		return imagePixel;
	}

	@Override
	public void robotConnected(Robot robot) {
		robotPositionChanged(robot);
	}

	@SuppressWarnings("serial")
	@Override
	public JPanel getPanel() {

		final ImageCameraSimulated ims = this;
		panel = new JPanel() {
			@Override
			protected void paintComponent(Graphics g) {
				super.paintComponent(g);
				if (ims.pointsCalculated)
					ims.paint();
			}
		};

		return panel;
	}

	@Override
	public void run() {
	}

	@Override
	public void worldChanged(World world) {

		ep.setTarget(world.getTargetPose());
		if (robotPose != null) {
			Matrix4 cameraPose = ep.getCamParameters().getPose();
			Matrix4 pose = new Matrix4();
			pose.mul(robotPose, cameraPose);
			calculateImageCoordinates(pose);
			if (this.panel != null) {
				// Vector2[] points = new Vector2[this.imagePointList.length]();
				Vector2[] points = this.imagePointList;
				ImagePointListList.add(points);
				paint();
			}
			notifyFeaturePointsChange(this);
		} else
			LogRecorder
					.log(LogLevel.ERROR,
							"The camera simulation cannot calculate the image till connecting the robot (I need to know the end-effector position) ");
	}

	@Override
	public String getPanelTitle() {
		return "Simulated camera";
	}

	@Override
	public int panelWidth() {
		return 640;
	}

	@Override
	public int panelHeight() {
		return 480;
	}

}
